
/************************************************************************************************* 

	Name: SONAR_C
	Autor: JA&AP
	
	Contains: 
				File containing all the functions related to the IRSensors hardware.
				
				
**************************************************************************************************/


#include <avr/io.h>
#include <avr/interrupt.h>

#include "sonar.h"
#include "uart.h"
#define BAUD_SONAR 9600

//Configura el puerto serial por el que se comunicara con los sonares
void sonar_init(){

	//inicializar el puerto serial. 9600 Bauds, no paridad, 2 bit de stop
	uart_init(UART_BAUD_SELECT_DOUBLE_SPEED(BAUD_SONAR,F_CPU),UART_FORMAT_8D0P2S);

}


void sonar_cleanBuffer()
{
	while(!(uart_getc() & UART_NO_DATA));
}

//envia peticion por puerto serial con la respectiva direccion del sonar
void sonar_write(unsigned char sonarNum, unsigned char units)
{
	
	//envio de direccion	
	uart_putc(sonarNum);
	//envio de comando. 
	switch(units)
	{
		case SONAR_INCH:
		uart_putc(0x50);
		break;
		
		case SONAR_CM:
		uart_putc(0x51);
		break;
		
		case SONAR_US:
		uart_putc(0x52);
		break;
		
		
	}
	

	
}

unsigned char sonar_read(unsigned char sonarNum, unsigned int *val, char newState)
{
	static char state = SONAR_STATE0_REQ;
	unsigned char lowVal;
	static unsigned char highVal;
	unsigned int rx;
	
	if(newState != -1)
		state = newState;

	switch(state)
	{
		case SONAR_STATE0_REQ:
			//envio de direccion	
			uart_putc(sonarNum);
			//envio de comando.
			uart_putc(0x5E);
			state++;
		break;
		case SONAR_STATE1_1B:
			rx = uart_getc();
			if(!(rx & UART_NO_DATA))
			{
				highVal = rx&0x00FF;
				
				state++;
			}
		break;
		case SONAR_STATE2_2B:
			rx = uart_getc();
			if(!(rx & UART_NO_DATA))
			{
				lowVal = rx&0x00FF;
				state = SONAR_STATE0_REQ;
				*val = (unsigned int)(highVal<<8) | (unsigned int)lowVal;
				
				return SONAR_READ_READY;
			}
		break;
	}
	
	return SONAR_READ_NOT_READY;
	
}


